/*
	@author Okan Aşık
	
*/
#include <libplayerc++/playerc++.h>
#include <iostream>
#include <cstring>

#include "args.h"

#define RAYS 32

int
main(int argc, char **argv)
{
  parse_args(argc,argv);
	
try {
  using namespace PlayerCc;

    PlayerClient robot(gHostname, gPort);
    Graphics2dProxy gp(&robot, gIndex);
		Position2dProxy pos(&robot, gIndex);
		
		LocalizeProxy loc(&robot, gIndex);

    std::cout << robot << std::endl;
		
		
		
		for(;;)
		{
			//std::cout<<"I am in"<<std::endl;
			// this blocks until new data comes; 10Hz by default
			robot.Read();
			
			loc.GetParticles();
			
			//playerc_localize_t loc_data;
			//loc.GetParticles(loc_data);
			
			double scale = loc.GetMapScale();
			std::cout << loc;
			
			int particle_num = loc.GetNumParticles();
			std::cout<<"particle_num " << particle_num <<std::endl;
			
			for (unsigned int i=0; i < particle_num; i++) {
				player_pose2d_t pose = loc.GetParticlePose(i);
				
				std::cout<<"posex: " << pose.px <<" posey: " << pose.py << std::endl;
				
				player_point_2d_t points[1];
				
				//double x_prime = pos.GetX
				//points[0].px = scale * pose.px;
				//points[0].py = scale * pose.py;
				points[0].px = pose.px;
				points[0].py = pose.py;
				//points[0].px = i/50;
				//points[0].py = 0;
				
				gp.Color( 255,0,0,0 );
				gp.DrawPoints( points, 1 );
				
				usleep(1000);
			}
			
			usleep(500000);
			gp.Clear();
			
			
			
			/*
			for (unsigned int i = 0; i < loc.GetNumHypoths(); ++i)
			{
				player_point_2d_t points[1];
				points[0].px = scale * loc.GetHypoth(i).mean.px;
				points[0].py = scale * loc.GetHypoth(i).mean.py;
				
				gp.Color( 255,0,0,0 );
				gp.DrawPoints( points, 1 );
				
				usleep(50000);
				
				//os << i << " (weight " << c.GetHypoth(i).alpha << "): ";
				//os << "[" << c.GetHypoth(i).mean << "]" << std::endl;
			}
			*/
			
			//usleep(100000);
			
			// get map scale
			/*
			
			double scale = loc.GetMapScale();
			int particle_num = loc.GetNumHypoths();
			std::cout<<"particle_num " << particle_num <<std::endl;
			
				for(int i=0; i<particle_num; i++) {
					player_pose2d_t pose = loc.GetParticlePose(i);
					std::cout << i << " x " << pose.px;
					std::cout << i << " y " << pose.py;
					player_point_2d_t points[1];
					points[0].px = scale * pose.px;
					points[0].py = scale * pose.py;
					
					gp.Color( 255,0,0,0 );
					gp.DrawPoints( points, 1 );
				}			
				*/
		}
		
/*
    player_point_2d_t pts[RAYS];

    double r;
    for( r=0; r<1.0; r+=0.05 )
		{
				int p;
				for( p=0; p<RAYS; p++ )
				{
						pts[p].px = r * cos(p * M_PI/(RAYS/2));
						pts[p].py = r * sin(p * M_PI/(RAYS/2));
				}

				gp.Color( 255,0,0,0 );
				gp.DrawPoints( pts, RAYS );

				usleep(500000);

				gp.Color( (int)(255.0*r),(int)(255-255.0*r),0,0 );
				gp.DrawPolyline( pts, RAYS/2 );

		}

    usleep(1000000);

    player_color_t col;
    memset( &col, 0, sizeof(col));

    for( r=1; r>0; r-=0.1 )
		{
				col.blue = (int)(r * 255.0);
				col.red  = (int)(255.0 - r * 255.0);

				player_point_2d_t pts[4];
				pts[0].px = -r;
				pts[0].py = -r;
				pts[1].px = r;
				pts[1].py = -r;
				pts[2].px = r;
				pts[2].py = r;\
				pts[3].px = -r;
				pts[3].py = r;

				gp.DrawPolygon( pts, 4, 1, col);

				usleep(300000);
		}

		usleep(1000000);

  //gp.Clear();
	*/
}
catch(PlayerCc::PlayerError e)
	{
		std::cerr << e << std::endl;
		return -1;
	}
	
  return 0;
}
